#include "config.h"
#include "common.h"
#include <string.h>
#include <string>
#include <sstream>
#include <algorithm>

CConfig::CConfig(char* path) {
	LoadConfig(path);
}

void CConfig::LoadConfig(char* path) {
	Loadprcopt(path);
	// LoadSites(path);
}

void CConfig::reset() {
	opt_.navsys_ = SYS_NONE;
	opt_.mode_ = MODE_SINGLE;
	opt_.freqtype_ = FREQTYPE_L1;
	opt_.freqnum_ = 1;
	opt_.elecutoff_ = 0;
}

void CConfig::Loadprcopt(char* path) {
	char path_conf[20480];
	strcpy(path_conf, path);
	strcat(path_conf, NAME_CONF);

	ifstream in(path_conf);
	if (!in) {
		cerr << "open configuration file error! " << endl;
		exit(-1);
	}
	while (!in.eof()) {
		string line;
		getline(in, line);
		ParseConfLine(line);
	}

}

void CConfig::ParseConfLine(string line) {
	int eqpos = -1, commentpos = -1;
	string label, value;
	
	for (int i = 0; i < line.size(); ++i) {
		if (i == 0 && line[i] == '#') return;
		if (line[i] == '=') eqpos = i;
		if (line[i] == '#') commentpos = i;
	}
	if (eqpos == -1) return;
	label = line.substr(0, eqpos);
	if (commentpos == 0) commentpos = line.size();
	value = line.substr(eqpos + 1, commentpos - eqpos - 1);
	auto itor = remove_if(label.begin(), label.end(), ::isspace);
	label.erase(itor, label.end());
	itor = remove_if(value.begin(), value.end(), ::isspace);
	value.erase(itor, value.end());
	if (!ParseLabel(label, value)) {
		cout << "FATAL: UNSUPPORTED OPTIONS" << endl;
		exit(-1);
	}
}

bool CConfig::ParseLabel(string label, string value) {
    bool issuccess = true;
    if (label == "gravity")    return SetGravity(value);
    else if (label == "alignment_time") return SetAlignTime(value);
    else if (label == "sampling_rate")  return SetSampleRate(value);
    else if (label == "init_x")   return SetInitialX(value);
    else if (label == "init_y")   return SetInitialY(value);
    else if (label == "init_z")   return SetInitialZ(value);
    else if (label == "init_vx")   return SetVx(value);
    else if (label == "init_vy")   return SetVy(value);
    else if (label == "init_vz")   return SetVz(value);
    else if (label == "lever_arm_forward")   return SetLeverForward(value);
    else if (label == "lever_arm_right")   return SetLeverRight(value);
    else if (label == "lever_arm_down")   return SetLeverDown(value);
    else if (label == "sigma_ra")   return SetSigmaRa(value);
    else if (label == "sigma_rg")   return SetSigmaRg(value);
    else if (label == "sigma_bad")   return SetSigmaBad(value);
    else if (label == "sigma_bgd")   return SetSigmaBgd(value);
    else if (label == "deltapos_flat")   return SetDeltaPosFlat(value);
    else if (label == "deltapos_vertical")   return SetDeltaPosVertical(value);
    else if (label == "deltavel_flat")   return SetDeltaVelFlat(value);
    else if (label == "deltavel_vertical")   return SetDeltaVelVertical(value);
    else if (label == "delta_pitch")   return SetDeltaPitch(value);
    else if (label == "delta_roll")   return SetDeltaRoll(value);
    else if (label == "delta_yaw")   return SetDeltaYaw(value);
    else if (label == "delta_acc")   return SetDeltaAcc(value);
    else if (label == "delta_gyro")   return SetDeltaGyro(value);
	/* not uesd **************************************************************
    else if (label == "Output_form_Accel")  return SetAccelOutMode(value);
    else if (label == "Output_form_gyro")   return SetGyroOutMode(value);
	* ***********************************************************************/
    else {
        cout << "unsupported options: " << label << endl;
        return false;
    }
}

bool CConfig::SetGravity(string label) {
    double gravity = str2num<double>(label);
    opt_.gravity_ = gravity;
    return true;
}

bool CConfig::SetAlignTime(string label) {
    double time = str2num<double>(label);
    opt_.align_time_ = time;
    return true;
}

bool CConfig::SetSampleRate(string label) {
    double rate = str2num<double>(label);
    opt_.sampling_rate_ = rate;
    return true;
}

bool CConfig::SetInitialX(string label) {
    double X = str2num<double>(label);
    opt_.init_pos_[0] = X;
    return true;
}

bool CConfig::SetInitialY(string label) {
    double Y = str2num<double>(label);
    opt_.init_pos_[1] = Y;
    return true;
}

bool CConfig::SetInitialZ(string label) {
    double Z = str2num<double>(label);
    opt_.init_pos_[2] = Z;
    return true;
}

bool CConfig::SetGyroOutMode(string label) {
    unsigned short mode = str2num<unsigned short>(label);
    opt_.gyro_output_ = mode;
    return true;
}

bool CConfig::SetAccelOutMode(string label) {
    unsigned short mode = str2num<unsigned short>(label);
    opt_.accel_output_ = mode;
    return true;
}

bool CConfig::SetVx(string label) {
	try {
		double velx = str2num<double>(label);
		opt_.init_vel_[0] = velx;
	}
	catch (...) {
		cout << "WARNING: VEL_X ERROR" << endl;
	}
	return true;
}

bool CConfig::SetVy(string label) {
	try {
		double vely = str2num<double>(label);
		opt_.init_vel_[1] = vely;
	}
	catch (...) {
		cout << "WARNING: VEL_Y ERROR" << endl;
	}
	return true;
}

bool CConfig::SetVz(string label) {
	try {
		double velz = str2num<double>(label);
		opt_.init_vel_[2] = velz;
	}
	catch (...) {
		cout << "WARNING: VEL_Z ERROR" << endl;
	}
	return true;
}

bool CConfig::SetLeverForward(string label) {
	try {
		double forward = str2num<double>(label);
		opt_.lever_[0] = forward;
	}
	catch (...) {
		cout << "WARNING: LEVER_FORWARD ERROR" << endl;
	}
	return true;
}

bool CConfig::SetLeverRight(string label)
{
	try {
		double right = str2num<double>(label);
		opt_.lever_[1] = right;
	}
	catch (...) {
		cout << "WARNING: LEVER_FORWARD ERROR" << endl;
	}
	return true;
}

bool CConfig::SetLeverDown(string label)
{
	try {
		double down = str2num<double>(label);
		opt_.lever_[2] = down;
	}
	catch (...) {
		cout << "WARNING: LEVER_FORWARD ERROR" << endl;
	}
	return true;
}

bool CConfig::SetSigmaRa(string label)
{
	double sigmaRa = str2num<double>(label);
	opt_.sigma_ra_ = sigmaRa;
	return true;
}

bool CConfig::SetSigmaRg(string label)
{
	double sigmaRg = str2num<double>(label);
	opt_.sigma_rg_ = sigmaRg;
	return true;
}

bool CConfig::SetSigmaBad(string label)
{
	double sigmaBad = str2num<double>(label);
	opt_.sigma_bad_ = sigmaBad;
	return true;
}

bool CConfig::SetSigmaBgd(string label)
{
	double sigma_gbd = str2num<double>(label);
	opt_.sigma_gbd_ = sigma_gbd;
	return true;
}

bool CConfig::SetDeltaPosFlat(string label)
{
	double deltaposflat = str2num<double>(label);
	opt_.deltapos_flat_ = deltaposflat;
	return true;
}

bool CConfig::SetDeltaPosVertical(string label)
{
	double deltaposver = str2num<double>(label);
	opt_.deltapos_vertical_ = deltaposver;
	return true;
}

bool CConfig::SetDeltaVertical(string label)
{
	double deltavertical = str2num<double>(label);
	opt_.deltapos_vertical_ = deltavertical;
	return true;
}

bool CConfig::SetDeltaVelFlat(string label)
{
	double deltavelflat = str2num<double>(label);
	opt_.deltavel_flat_ = deltavelflat;
	return true;
}

bool CConfig::SetDeltaVelVertical(string label)
{
	double deltavelver = str2num<double>(label);
	opt_.deltavel_vertical_ = deltavelver;
	return true;
}

bool CConfig::SetDeltaPitch(string label) {
	double deltapitch = str2num<double>(label);
	opt_.delta_pitch_ = deltapitch;
	return true;
}

bool CConfig::SetDeltaRoll(string label)
{
	double deltaroll = str2num<double>(label);
	opt_.delta_roll_ = deltaroll;
	return true;
}

bool CConfig::SetDeltaYaw(string label)
{
	double deltayaw = str2num<double>(label);
	opt_.delta_yaw_ = deltayaw;
	return true;
}

bool CConfig::SetDeltaAcc(string label)
{
	double deltaAcc = str2num<double>(label);
	opt_.delta_acc_ = deltaAcc;
	return true;
}

bool CConfig::SetDeltaGyro(string label)
{
	double deltagyro = str2num<double>(label);
	opt_.delta_gyro_ = deltagyro;
	return true;
}


bool CConfig::SetSys(string value) {
	opt_.navsys_ = SYS_NONE; opt_.nsys_ = 0;
	for (int i = 0; i < value.size(); ++i) {
		if (value[i] == 'G') {
			opt_.navsys_ |= SYS_GPS; opt_.nsys_++;
		}
		if (value[i] == 'C') {
			opt_.navsys_ |= SYS_BDS; opt_.nsys_++;
		}
	}
	if (opt_.navsys_ == SYS_NONE) {
		cout << "no navigation system input" << endl;
		return false;
	}
	return true;
}

bool CConfig::SetCutOff(string value) {
	stringstream buff;
	double elev;
	buff << value; buff >> elev;
	if (elev < 1e-3) return false;
	opt_.elecutoff_ = Deg2Rad(elev);
	return true;
}

bool CConfig::SetMode(string value) {
	transform(value.begin(), value.end(), value.begin(), ::tolower);
	if (value == "single")  opt_.mode_ = MODE_SINGLE;
	else if (value == "rtk") opt_.mode_ = MODE_RTK;
	else return false;

	return true;
}

bool CConfig::SetEphType(string value) {
	if (value == "brdc")    opt_.ephtype_ = EPH_BRDC;
	else if (value == "prec") opt_.ephtype_ = EPH_PREC;
	else return false;

	return true;
}

bool CConfig::SetClkType(string value) {
	if (value == "brdc")    opt_.clktype_ = EPH_BRDC;
	else if (value == "prec") opt_.clktype_ = EPH_PREC;
	else return false;

	return true;
}

bool CConfig::SetFreq(string value) {
	opt_.freqtype_ = 0; opt_.freqnum_ = 0;
	for (int i = 0; i < value.size(); ++i) {
		if (value[i] == '1') {
			opt_.freqtype_ |= FREQTYPE_L1;
			opt_.freqnum_++;
		}
		else if (value[i] == '2') {
			opt_.freqtype_ |= FREQTYPE_L2;
			opt_.freqnum_++;
		}
		else if (value[i] == '3') {
			opt_.freqtype_ |= FREQTYPE_L3;
			opt_.freqnum_++;
		}
		else if (value[i] == ',') continue;
		else if (value[i] == 'L') continue;
		else return false;
	}
	return true;
}

void CConfig::LoadSites(char* path) { // may be not used in LC project, reserved
	strcat(path, NAME_SITE);

	ifstream in(path);
	if (!in) {
		cout << "can not find sites configurations " << endl;
		return;
	}
	while (!in.eof()) {
		string line;
		getline(in, line);
		ParseSiteLine(line);
	}
	in.close();
}

void CConfig::ParseSiteLine(string line) {
	if (line[0] != ' ') return;
	static int sitenum = 0;
	stringstream buff;
	buff << line;
	if (sitenum == 0) {
		buff >> opt_.nbase_;
		opt_.nbase_ = opt_.nbase_.substr(0, 4);
		buff >> opt_.base_[0] >> opt_.base_[1] >> opt_.base_[2];
	}
	else {
		buff >> opt_.nrover_;
		opt_.nrover_ = opt_.nrover_.substr(0, 4);
		buff >> opt_.rover_[0] >> opt_.rover_[1] >> opt_.rover_[2];
	}
	sitenum++;
	opt_.sitenum_ = sitenum;
}

prcopt CConfig::GetConf() {
	return opt_;
}